#ifndef STX_ALGORITHM_STABLE_PARTITION_HPP
#define STX_ALGORITHM_STABLE_PARTITION_HPP

#include <stx/algorithm/common.hpp>
#include <stx/algorithm/swap.hpp>
#include <stx/algorithm/advance.hpp>
#include <stx/algorithm/rotate.hpp>
#include <stx/algorithm/distance.hpp>
#include <stx/core/scoped_ptr.hpp>

namespace stx {

template <class Predicate, class ForwardIterator, class Distance, class StdPair>
inline ForwardIterator
stable_partition_impl(
    ForwardIterator first,
    ForwardIterator last,
    Predicate pred,
    Distance len,
    StdPair p,
    std::forward_iterator_tag fit)
{
    // *first is known to be false
    // len >= 1
    if (len == 1) {
        return first;
    }
    if (len == 2) {
        ForwardIterator m = first;
        if (pred(*++m)) {
            swap(*first, *m);
            return m;
        }
        return first;
    }
    if (len <= p.second) {
        // The buffer is big enough to use
        typedef typename std::iterator_traits<ForwardIterator>::value_type value_type;
        destruct_n d(0);
        scoped_ptr<value_type, destruct_n&> h(p.first, d);
        // Move the falses into the temporary buffer, and the trues to the front of the line
        // Update first to always point to the end of the trues
        value_type* t = p.first;
        ::new(t) value_type(stx::move(*first));
        d.incr((value_type*)0);
        ++t;
        ForwardIterator i = first;
        while (++i != last) {
            if (pred(*i)) {
                *first = stx::move(*i);
                ++first;
            } else {
                ::new(t) value_type(stx::move(*i));
                d.incr((value_type*)0);
                ++t;
            }
        }
        // All trues now at start of range, all falses in buffer
        // Move falses back into range, but don't mess up first which points to first false
        i = first;
        for (value_type* _T2 = p.first; _T2 < t; ++_T2, ++i) {
            *i = stx::move(*_T2);
        }
        // h destructs moved-from values out of the temp buffer, but doesn't deallocate buffer
        return first;
    }
    // Else not enough buffer, do in place
    // len >= 3
    ForwardIterator m = first;
    Distance len2 = len / 2;  // len2 >= 2
    stx::advance(m, len2);
    // recurse on [first, m), *first know to be false
    // F?????????????????
    // f       m         l
    typedef typename add_lvalue_reference<Predicate>::type pred_ref;
    ForwardIterator first_false = stable_partition_impl<pred_ref>(first, m, pred, len2, p, fit);
    // TTTFFFFF??????????
    // f  ff   m         l
    // recurse on [m, last], except increase m until *(m) is false, *last know to be true
    ForwardIterator m1 = m;
    ForwardIterator second_false = last;
    Distance len_half = len - len2;
    while (pred(*m1)) {
        if (++m1 == last) {
            goto second_half_done_;
        }
        --len_half;
    }
    // TTTFFFFFTTTF??????
    // f  ff   m  m1     l
    second_false = stable_partition_impl<pred_ref>(m1, last, pred, len_half, p, fit);
second_half_done_:
    // TTTFFFFFTTTTTFFFFF
    // f  ff   m    sf   l
    return stx::rotate(first_false, m, second_false);
    // TTTTTTTTFFFFFFFFFF
    //         |
}

template <class Predicate, class ForwardIterator>
inline ForwardIterator
stable_partition_impl(
    ForwardIterator first,
    ForwardIterator last,
    Predicate pred,
    std::forward_iterator_tag)
{
    const unsigned alloc_limit = 3;  // might want to make this a function of trivial assignment
    // Either prove all true and return first or point to first false
    while (1) {
        if (first == last) {
            return first;
        }
        if (!pred(*first)) {
            break;
        }
        ++first;
    }
    // We now have a reduced range [first, last)
    // *first is known to be false
    typedef typename std::iterator_traits<ForwardIterator>::difference_type difference_type;
    typedef typename std::iterator_traits<ForwardIterator>::value_type value_type;
    difference_type len = stx::distance(first, last);
    std::pair<value_type*, std::ptrdiff_t> p(0, 0);
    scoped_ptr<value_type, return_temporary_buffer_impl> h;
    if (len >= alloc_limit) {
        p = stx::get_temporary_buffer<value_type>(len);
        h.reset(p.first);
    }
    return stable_partition_impl<typename add_lvalue_reference<Predicate>::type>(
        first,
        last,
        pred,
        len,
        p,
        std::forward_iterator_tag());
}

template <class Predicate, class BidirectionalIterator, class Distance, class StdPair>
inline BidirectionalIterator
stable_partition_impl(
    BidirectionalIterator first,
    BidirectionalIterator last,
    Predicate pred,
    Distance len,
    StdPair p,
    std::bidirectional_iterator_tag bit)
{
    // *first is known to be false
    // *last is known to be true
    // len >= 2
    if (len == 2) {
        swap(*first, *last);
        return last;
    }
    if (len == 3) {
        BidirectionalIterator m = first;
        if (pred(*++m)) {
            swap(*first, *m);
            swap(*m, *last);
            return last;
        }
        swap(*m, *last);
        swap(*first, *m);
        return m;
    }
    if (len <= p.second) {
        // The buffer is big enough to use
        typedef typename std::iterator_traits<BidirectionalIterator>::value_type value_type;
        destruct_n d(0);
        scoped_ptr<value_type, destruct_n&> h(p.first, d);
        // Move the falses into the temporary buffer, and the trues to the front of the line
        // Update first to always point to the end of the trues
        value_type* t = p.first;
        ::new(t) value_type(stx::move(*first));
        d.incr((value_type*)0);
        ++t;
        BidirectionalIterator i = first;
        while (++i != last) {
            if (pred(*i)) {
                *first = stx::move(*i);
                ++first;
            } else {
                ::new(t) value_type(stx::move(*i));
                d.incr((value_type*)0);
                ++t;
            }
        }
        // move *last, known to be true
        *first = stx::move(*i);
        i = ++first;
        // All trues now at start of range, all falses in buffer
        // Move falses back into range, but don't mess up first which points to first false
        for (value_type* _T2 = p.first; _T2 < t; ++_T2, ++i) {
            *i = stx::move(*_T2);
        }
        // h destructs moved-from values out of the temp buffer, but doesn't deallocate buffer
        return first;
    }
    // Else not enough buffer, do in place
    // len >= 4
    BidirectionalIterator m = first;
    Distance len2 = len / 2;  // len2 >= 2
    stx::advance(m, len2);
    // recurse on [first, m-1], except reduce m-1 until *(m-1) is true, *first know to be false
    // F????????????????T
    // f       m        l
    BidirectionalIterator m1 = m;
    BidirectionalIterator first_false = first;
    Distance len_half = len2;
    while (!pred(*--m1)) {
        if (m1 == first) {
            goto first_half_done_;
        }
        --len_half;
    }
    // F???TFFF?????????T
    // f   m1  m        l
    typedef typename add_lvalue_reference<Predicate>::type pred_ref;
    first_false = stable_partition_impl<pred_ref>(first, m1, pred, len_half, p, bit);
first_half_done_:
    // TTTFFFFF?????????T
    // f  ff   m        l
    // recurse on [m, last], except increase m until *(m) is false, *last know to be true
    m1 = m;
    BidirectionalIterator second_false = last;
    ++second_false;
    len_half = len - len2;
    while (pred(*m1)) {
        if (++m1 == last) {
            goto second_half_done_;
        }
        --len_half;
    }
    // TTTFFFFFTTTF?????T
    // f  ff   m  m1    l
    second_false = stable_partition_impl<pred_ref>(m1, last, pred, len_half, p, bit);
second_half_done_:
    // TTTFFFFFTTTTTFFFFF
    // f  ff   m    sf  l
    return stx::rotate(first_false, m, second_false);
    // TTTTTTTTFFFFFFFFFF
    //         |
}

template <class Predicate, class BidirectionalIterator>
inline BidirectionalIterator
stable_partition_impl(
    BidirectionalIterator first,
    BidirectionalIterator last,
    Predicate pred,
    std::bidirectional_iterator_tag)
{
    typedef typename std::iterator_traits<BidirectionalIterator>::difference_type difference_type;
    typedef typename std::iterator_traits<BidirectionalIterator>::value_type value_type;
    const difference_type alloc_limit = 4;  // might want to make this a function of trivial assignment
    // Either prove all true and return first or point to first false
    while (1) {
        if (first == last) {
            return first;
        }
        if (!pred(*first)) {
            break;
        }
        ++first;
    }
    // first points to first false, everything prior to first is already set.
    // Either prove [first, last) is all false and return first, or point last to last true
    do {
        if (first == --last) {
            return first;
        }
    } while (!pred(*last));
    // We now have a reduced range [first, last]
    // *first is known to be false
    // *last is known to be true
    // len >= 2
    difference_type len = stx::distance(first, last) + 1;
    std::pair<value_type*, std::ptrdiff_t> p(0, 0);
    scoped_ptr<value_type, return_temporary_buffer_impl> h;
    if (len >= alloc_limit) {
        p = stx::get_temporary_buffer<value_type>(len);
        h.reset(p.first);
    }
    return stable_partition_impl<typename add_lvalue_reference<Predicate>::type>(
        first,
        last,
        pred,
        len,
        p,
        std::bidirectional_iterator_tag());
}

template <class ForwardIterator, class Predicate>
inline ForwardIterator
stable_partition(ForwardIterator first, ForwardIterator last, Predicate pred)
{
    return stable_partition_impl<typename add_lvalue_reference<Predicate>::type>(
        first,
        last,
        pred,
        typename std::iterator_traits<ForwardIterator>::iterator_category());
}

} // namespace stx

#endif // STX_ALGORITHM_STABLE_PARTITION_HPP
